package sensors.virtualSensors;

import java.util.Map;

import maze.Location;
import robots.OwnGhost;
import robots.VirtualGhost;
import sensors.UltrasonicSensor;
import simulation.Raycaster;
import simulation.Simulator;

public class VirtualUltrasonicSensor extends VirtualSensor implements UltrasonicSensor{
	
	public VirtualUltrasonicSensor(VirtualGhost ghost, Simulator sim, boolean randomValues){
		super(ghost, sim, randomValues);
	}

	@Override
	public int getDistance() {
		return SimulateUltrasonicSensor();
	}
	private int SimulateUltrasonicSensor()
	{
		Simulator sim=getSimulator();
		OwnGhost ghost = getGhost();
		int distToWall =  Raycaster.cast(sim.getMaze(),sim.getGhostLocations().get(ghost), sim.getGhostRotations().get(ghost)+ghost.getSensorRotation());
		if(randomizer != null)
			distToWall = randomizer.ultrasonicRandom(distToWall);
		int distToRobot = UltrasonicRobotCheck(ghost,20);
		return Math.min(distToWall, distToRobot);
	}
	/**
	 * checks if there is an other ghost between him and the closest wall and returns the distance;
	 * 
	 * @param sensorWidth
	 * 			| the angle of effect that the sensor can see
	 * @return
	 * 		|the minimum of the distance between him and the closest wall and him and the closest robot.
	 */
	private int UltrasonicRobotCheck(OwnGhost ghost,int sensorWidth){
		double closest = Integer.MAX_VALUE; //yes still integer because we don't want an overflow when converting back
		Map<OwnGhost, Location> ghostLocations = getSimulator().getGhostLocations();
		Map<OwnGhost, Integer> ghostRotations = getSimulator().getGhostRotations();
		Location location = ghostLocations.get(ghost);
		int sensorangle = ghostRotations.get(ghost)+ghost.getSensorRotation();
		for (OwnGhost other : ghostLocations.keySet()) {
			if(!other.equals(ghost)){
				Location locOther = ghostLocations.get(other);
				int angle = (int) Math.toDegrees(Math.atan2((locOther.y-location.y),(locOther.x-location.x)));
				//check if angle is in the sensorrange
				if(Math.abs(angle-sensorangle) <= sensorWidth){
					/*//to avoid division by zero
					if (angle == 0 || angle == 180 || angle == -180)
						angle += 1;
					int dist = (int) Math.abs(((locOther.y-location.y)/Math.sin(Math.toRadians(angle))));*/
					
					//how about the following way to calcvulate distance?
					double dist = ((locOther.x-location.x)*(locOther.x-location.x) + (locOther.y-location.y)*(locOther.y-location.y));
					//we'll sqrt it later
					if (dist<closest){
						closest = dist;
					}
				}
			}
		}
		return (int)Math.sqrt(closest);
	}
}
